#include "task_manager.h"

void TaskManager::getLoopTask(geometry_msgs::Pose &pose)
{
    if(current_route.route_line_.empty())
    {
        task_state = FINISHED;
	return;
    }
    if(current_route.loop_count_ == 0)        //loop forever
    {
        if(current_route.current_point_ >= current_route.points_num_)
        {
            current_route.current_point_ = 0;
	    if(got_new_route)
	    {
                task_state = FINISHED;
                return ;		//get new route, stop current route task if get final point
            }
	    if(!task_valid_)
	    {
                task_state = CANCELED;
                return ;		//cancel task
            }
        }
        pose = current_route.route_line_[current_route.current_point_];
        current_route.current_point_++;
        task_state = TASK_OK;
    }
    else                                //loop for certain circle
    {
        if(current_route.current_loop_count_<current_route.loop_count_)             //loop go on
        {
            if(current_route.current_point_ >= current_route.points_num_)           //go to next loop
            {                
	        if(!task_valid_)
	        {
                    task_state = CANCELED;
                    return ;		//get new route, stop current route task if get final point
                }
                current_route.current_loop_count_++;
                if(current_route.current_loop_count_ >= current_route.loop_count_)  //next loop do not exist, task finished
                {
                    task_state = FINISHED;
                }
                else{                                   //execute next loop 
                    current_route.current_point_ = 0;
                    pose = current_route.route_line_[current_route.current_point_];
                    current_route.current_point_++;
                    task_state = TASK_OK; 
                }                
            }
            else                 //current loop go on
            {
                pose = current_route.route_line_[current_route.current_point_];
                current_route.current_point_++;
                task_state = TASK_OK;
            } 
        }
        else                    //loop over, task finished
        {
            task_state = FINISHED;
        }        
    }
    if(task_state == TASK_OK)
    {
	tf::Quaternion quat;
        tf::quaternionMsgToTF(pose.orientation,quat);
        double roll,pitch,yaw;
        tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
//        ROS_INFO("goal position:(%f,%f,%f)",pose.position.x,pose.position.y,yaw);
    }
}
void TaskManager::getNoloopTask(geometry_msgs::Pose &pose)
{
    if(current_route.route_line_.empty())
    {
        task_state = FINISHED;
	return;
    }
    if(current_route.current_point_ < current_route.points_num_)
    {
        pose = current_route.route_line_[current_route.current_point_];
        current_route.current_point_++;
        task_state = TASK_OK;
    }
    else
    {
        task_state = FINISHED;
    }
}
taskState TaskManager::getGoal(move_base_msgs::MoveBaseGoal &goal)
{
    //if(!task_valid_)
    //    return CANECLED;
    geometry_msgs::Pose pose;
    mutex_.lock();
    if(got_new_route && task_state != TASK_OK)
    {
        current_route = new_route;
        got_new_route = false;
//	ROS_INFO("get new route");
    }
    switch(current_route.mode_)
    {
        case RECORD:
            break;
        case HANDWRITE:
            break;
        case MAKEUP:
            if(current_route.loop_)
                getLoopTask(pose);
            else
                getNoloopTask(pose);           
            break;
        case POINT:
            break;
    }
    mutex_.unlock();
    if(task_state == TASK_OK)
    {
        goal.target_pose.header.frame_id = "map";
	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.pose = pose;
    }
    return task_state;
}
bool TaskManager::cancelTask()
{
    mutex_.lock();
    task_valid_ = false;
    mutex_.unlock();
    return true;
}

bool TaskManager::setRouteLines(vector<string> route_line_names,\
        vector<vector<geometry_msgs::Pose> > route_lines)
{
    route_line_names_ = route_line_names;
    route_lines_ = route_lines;
    
    if(route_line_names.size() == route_lines.size())
    {
        if(route_line_names.size()>0)
        {
            return true;
        }
        else 
        {
            return false; 
        }
    }
    else
    {
        return false;
    }
    return true;
}
bool TaskManager::findNextTask(Task &task)
{
    time_t now = time(NULL);
    char now_time[9];
    strftime(now_time,sizeof(now_time),"%H:%M:%S",localtime(&now));
    string now_time_str = now_time;
    for(int i=0;i<task_list_.size();i++)
    {
        if(task_list_[i].start_time <= now_time_str && task_list_[i].end_time > now_time_str)
        {
            task = task_list_[i];
            return true;
        }
    }
    return false;
}
void TaskManager::executeTask(Task task)
{
    vector<vector<geometry_msgs::Pose> > route_line;
    vector<string>::iterator it = find(route_line_names_.begin(),\
        route_line_names_.end(),task.route_name);
    auto index = std::distance(std::begin(route_line_names_),it);
    //set new route, will be executed later
    mutex_.lock();
    new_route.name_ = task.route_name;
    new_route.current_loop_count_ = 0;
    new_route.current_point_ = 0;
    new_route.loop_ = task.loop;
    new_route.loop_count_ = task.loop_count;    
    new_route.route_line_ = route_lines_[index];
    new_route.points_num_ = route_lines_[index].size();
    new_route.mode_ = MAKEUP;
    got_new_route = true;
    task_valid_ = true;
    mutex_.unlock();

    robot_state = RUNNING;
    last_task = task;
    ROS_INFO("execute task:%s",task.task_name.c_str());
}
bool TaskManager::arriveGoal()
{    
    if(current_route.route_line_.empty())
    {
        return false;
    }
    int index = current_route.route_line_.size()-1;
    float dis_x = robot_pose_.position.x - current_route.route_line_[index].position.x;
    float dis_y = robot_pose_.position.y - current_route.route_line_[index].position.y;
    float dis_z = robot_pose_.position.z - current_route.route_line_[index].position.z;
    if(dis_x*dis_x + dis_y*dis_y + dis_z*dis_z < 1.0)
    {
        return true;
    }
    else
    {
        return false;
    }    
}
void TaskManager::schedule()
{
    ros::Rate loop(0.5);    
    while(ros::ok())
    {
        Task task;
        if(!findNextTask(task))
        {
//            ROS_INFO_STREAM_THROTTLE(1.0,"Do not find next task");
            cancelTask();
            loop.sleep();
            continue;
        }
        if(task.task_name == last_task.task_name)
        {
            if(task.loop == false && task_state == FINISHED)  
            {
                if(arriveGoal())
                {
                    loop.sleep();
                    continue;
                }
                else{       //robot did not arrive sentry point
                    executeTask(task);
                    loop.sleep();
                    continue;
                }
            }
            else
            {
                loop.sleep();
                continue;
            }
        }
        else                //new task
        {
            switch(robot_state)
            {
                case IDLE:
                    executeTask(task);
                    break;
                case RUNNING:
                    executeTask(task);
                    break;
                default:
                    break;
            }
        }
        loop.sleep();
    }
}
